/*
 * main.cpp
 *
 * Author: Kellen Carey
 *   Date: June 20, 2014
 */
#include "Vision.h"
#include <pthread.h>

int max_count = 65;
int max_turn_count = 1000;

double getUnixTime() {
	struct timespec tv;

	if (clock_gettime(CLOCK_REALTIME, &tv) != 0) {
		return 0;
	}

	return (((double)tv.tv_sec) + (tv.tv_nsec / 1000000000.0));
}

void doMotion(Vision &vis, MotorController &motorController) {
	bool done = false;
	double startTime = getUnixTime();
	double currentTime = getUnixTime();
	double stepTime;
	while (!done) {
		int step_result = motorController.step(false);
		switch (step_result) {
		case MUL8_STEP_NULL:
			break;
		case MUL8_STEP_FINISHED:
			// Loop through vision until it's time to execute the next motion
			startTime = getUnixTime();
			currentTime = getUnixTime();
			stepTime = motorController.getStepTime();
			stepTime -= 0.15;	// Subtract 150 mS to deal with frame processing time
			do {
				vis.nextFrame();
				currentTime = getUnixTime();
			} while ((currentTime-startTime) < stepTime);
			break;
		case MUL8_MOTION_FINISHED:
			done = true;
			break;
		}
	}
}

void search(Vision &vis) {
	bool position = false;
	bool done = false;

	vis.setAction(SEARCH_FOR_GOAL);
	//	vis.setAction(CENTER_BALL);

	while (!done) {
		if (!position && vis.knowsRobotPosition()) {
			vis.setAction(SEARCH_FOR_BALL);
			std::cout << "Searching for Ball" << std::endl;
			position = true;
		}
		else if (vis.knowsBallPosition()) {
			done = true;
		}

		int c = waitKey(1);

		if ((char)c == 27) {
			done = true;
		}

		vis.nextFrame();
	}
	// Robot knows where both the ball and the field are
	std::cout << "Vision knows where it is, and where the ball is.\n" <<
			"We would now normally wait for the robot to move," <<
			" and then update our world." << std::endl;
	std::cout << std::endl;
	std::cout << "The robot is located: (" << vis.getRobotX() <<
			", " << vis.getRobotY() << ") @ " << vis.getRobotTheta()
			<< std::endl;
	std::cout << "The ball is located: (" << vis.getBallX() << ", "
			<< vis.getBallY() << ")" << std::endl;
}
void turn(MotorController &motorController, Vision &vis, int theta){

	if(theta<0){
		//turn left
		if(theta<0 && theta>=-15){
			//between 0 and -15
			motorController.setMotion("Tl15");
			vis.updateRobotPosition(ODOMETRY_TL15_DISTANCE, ODOMETRY_TL15_THETA);
			vis.updateRobotTheta(ODOMETRY_TL15_TURN);
		}
		else if(theta<-15 && theta>=-30){
			//between -15 and -30
			motorController.setMotion("Tl30");
			vis.updateRobotPosition(ODOMETRY_TL30_DISTANCE, ODOMETRY_TL30_THETA);
			vis.updateRobotTheta(ODOMETRY_TL30_TURN);
		}
		else if(theta<-30 && theta>-45){
			//between -30 and -45
			motorController.setMotion("Tl45");
			vis.updateRobotPosition(ODOMETRY_TL45_DISTANCE, ODOMETRY_TL45_THETA);
			vis.updateRobotTheta(ODOMETRY_TL45_TURN);
		}
	}

	else if(theta>=0){
		if(theta>=0 && theta<15){
			//between 0 and 15
			motorController.setMotion("Tr15");
			vis.updateRobotPosition(ODOMETRY_TR15_DISTANCE, ODOMETRY_TR15_THETA);
			vis.updateRobotTheta(ODOMETRY_TR15_TURN);
		}
		else if(theta>=15 && theta<30){
			//between 15 and 30
			motorController.setMotion("Tr30");
			vis.updateRobotPosition(ODOMETRY_TR30_DISTANCE, ODOMETRY_TR30_THETA);
			vis.updateRobotTheta(ODOMETRY_TR30_TURN);
		}
		else if(theta>=30 && theta<45){
			//between 30 and 45
			motorController.setMotion("Tr30");
			vis.updateRobotPosition(ODOMETRY_TR30_DISTANCE, ODOMETRY_TR30_THETA);
			vis.updateRobotTheta(ODOMETRY_TR30_TURN);

		}
	}
	else{
		//do nothing?
	}
	doMotion(vis, motorController);
}
/*
 * This function returns true when it is near the ball, or false if it loses track of the ball.
 */
/*bool*/void walkTowardsBall(Vision &vis, MotorController &motorController, int distance_to_ball) {
	bool done = false;

	std::cout << "Walking towards the ball time" << std::endl;

	vis.setAction(CENTER_BALL);
	double temp_time = getUnixTime();
	// Search for ball for 6 seconds
	while ((getUnixTime()-temp_time) < 6) {
		vis.nextFrame();
	}
	temp_time = getUnixTime();


	double waitTimer = getUnixTime();
	// TODO I forgot how the robot's X and Y are oriented, might have to change some vision code if this doesn't work
	while (!done) {

		double turningTheta = motorController.getHeadAngle()*-1;
		double previous_distance = vis.getBallDistance();

		// TODO We assume here that each turn left motion turns us approximately 45 degrees
		if (turningTheta > 23) {
				std::cout << "Robot turning left. New theta = " << vis.getRobotTheta() << std::endl;
				if (motorController.getMotion() == "W1") {
					motorController.setMotion("W1i");
				}
				else if (motorController.getMotion() == "W2") {
					motorController.setMotion("W2i");
				}
				doMotion(vis, motorController);
				// Don't need to update odometry as these motions should not affect any odometry data
				motorController.setMotion("Tl30");
				doMotion(vis, motorController);
				vis.updateRobotPosition(ODOMETRY_TL30_DISTANCE, ODOMETRY_TL30_THETA);
				vis.updateRobotTheta(ODOMETRY_TL30_TURN);
		}
		// TODO We assume here that each turn right motion turns us approximately 45 degrees
		else if (turningTheta < -23) {
				std::cout << "Robot turning right. New theta = " << vis.getRobotTheta() << std::endl;
				if (motorController.getMotion() == "W1") {
					motorController.setMotion("W1i");
				}
				else if (motorController.getMotion() == "W2") {
					motorController.setMotion("W2i");
				}
				doMotion(vis, motorController);
				std::cout << "Setting motion to TR" << std::endl;
				motorController.setMotion("Tr30");
				doMotion(vis, motorController);
				vis.updateRobotPosition(ODOMETRY_TR30_DISTANCE, ODOMETRY_TR30_THETA);
				vis.updateRobotTheta(ODOMETRY_TR30_TURN);
				std::cout << "Robot turning right. New theta = " << vis.getRobotTheta() << std::endl;
		}
		// Here we assume that we are facing in the correct direction, so let's walk forward
		else if (previous_distance > distance_to_ball) { //
				std::string currMo = motorController.getMotion();
				if (currMo == "Wi") {
					std::cout << "Position W0_m" << std::endl;
					motorController.setMotion("W0_m");
					vis.updateRobotPosition(ODOMETRY_W0_M_DISTANCE, ODOMETRY_W0_M_THETA);
					vis.updateRobotTheta(ODOMETRY_W0_M_TURN);
					waitTimer = getUnixTime();
				}
				else if (currMo == "W0_m" && ((getUnixTime()-waitTimer) > 1)) {
					motorController.setMotion("W2");
					vis.updateRobotPosition(ODOMETRY_W2_DISTANCE, ODOMETRY_W2_THETA);
					vis.updateRobotTheta(ODOMETRY_W2_TURN);
					std::cout << "Position W2" << std::endl;
				}
				else if (currMo == "W1") {
					motorController.setMotion("W2");
					vis.updateRobotPosition(ODOMETRY_W2_DISTANCE, ODOMETRY_W2_THETA);
					vis.updateRobotTheta(ODOMETRY_W2_TURN);
					std::cout << "Position W2" << std::endl;
				}
				else if (currMo == "W2") {
					motorController.setMotion("W1");
					vis.updateRobotPosition(ODOMETRY_W1_DISTANCE, ODOMETRY_W1_THETA);
					vis.updateRobotTheta(ODOMETRY_W1_TURN);
					std::cout << "Position W1" << std::endl;
				}
				else {
					motorController.setMotion("Wi");
					// No need to update odometry, as no foot movements are involved
					std::cout << "Position Wi" << std::endl;
				}
				doMotion(vis, motorController);
				//				motorController.setMotion("Walk Forward");
				std::cout << "Robot walking forward. New coordinates: (" << vis.getRobotX() << ", " << vis.getRobotY() << ")" << std::endl;
				std::cout << "Distance to ball: " << previous_distance << std::endl;
		}
		else {
			// We are within 1 meter of the ball
			//should call getBehindBall();
			// At this point, we would determine how to move, whether to move to the far side of the ball, or walk forward and kick it

			std::string currMo = motorController.getMotion();
			if (currMo == "W2") {
				motorController.setMotion("W2i");
				std::cout << "Returning from Position W2" << std::endl;
			}
			else if (currMo == "W1") {
				motorController.setMotion("W1i");
				std::cout << "Returning from Position W1" << std::endl;
			}

			doMotion(vis, motorController);
			// No need to update odometry, as these motions should not affect any odometry data
			done = true;
		}
	}
}

void checkLocation(Vision &vis, MotorController &motorController) {
	vis.nextFrame();
	double robotTheta = vis.getRobotTheta();
	int motor23pos = motorController.getMotorPositionReadWord(23);
	int motor24pos = motorController.getMotorPositionReadWord(24);
	double robotHeadTheta = motorController.getHeadAngle();
	vis.setAction(WAIT);
	if (robotTheta < 0) {
		while (robotTheta < robotHeadTheta) {
			// MUL8 needs to turn head left in order to find the goal
			motorController.moveHead(MUL8_HEAD_UP_LEFT, 256);
			vis.nextFrame();
			robotHeadTheta = motorController.getHeadAngle();
		}
		motorController.stopHead();
	}
	else {
		while (robotHeadTheta < robotTheta) {
			// MUL8 needs to turn head right in order to find the goal
			motorController.moveHead(MUL8_HEAD_UP_RIGHT, 256);
			vis.nextFrame();
			robotHeadTheta = motorController.getHeadAngle();
		}
		motorController.stopHead();
	}

	// The head should be pointing towards the goal, so set the vision action to find the goal
	vis.setAction(SEARCH_FOR_GOAL);

	bool localized = false;

	while (!localized) {
		if (vis.getAction() == LOCALIZE_GOAL) {
			localized = true;
		}
		vis.nextFrame();
	}

	// This should update the robot's position estimation
	for (int i = 0; i < 20; i++) {
		vis.nextFrame();
	}


	double headAngle = motorController.getHeadAngle();
	std::cout << "Robot Head Theta: " << headAngle << std::endl;
	double robotAngle = vis.getRobotTheta();
	std::cout << "Robot Theta: " << robotAngle << std::endl;

	vis.updateRobotTheta(headAngle-robotAngle);

	motorController.setMotorPosition(23, motor23pos, 256);
	motorController.setMotorPosition(24, motor24pos, 256);
}


/*
 * This function returns true when it is behind the ball, or false if it loses sight of the ball or the ball moves too far away.
 */
/*bool*/void getBehindBall(Vision &vis, MotorController &motorController) {

	double theta = vis.getRobotTheta();

	std::cout << "Received Vision reference has theta of " << theta << std::endl;

	std::string motion;


	// If theta is negative, we have to side step right
	if (theta < -7) {
		motion = "SSr";
	}
	// If theta is positive, we have to side step left
	else {
		motion = "SSl";
	}

	// Need to keep side stepping and turning until our theta is zero
	while (theta < -12 || theta > 12 ) {
		motorController.setMotion(motion);
		doMotion(vis, motorController);
		if (motion == "SSr") {
			vis.updateRobotPosition(ODOMETRY_SSR_DISTANCE, ODOMETRY_SSR_THETA);
			vis.updateRobotTheta(ODOMETRY_SSR_TURN);
		}
		else {
			vis.updateRobotPosition(ODOMETRY_SSL_LONG_DISTANCE, ODOMETRY_SSL_LONG_THETA);
			vis.updateRobotTheta(ODOMETRY_SSL_LONG_TURN);
		}
		double robot_head_theta = motorController.getHeadAngle();
		while (robot_head_theta < -7) {
			turn(motorController, vis, robot_head_theta);
//			doMotion(vis, motorController);
			robot_head_theta = motorController.getHeadAngle();
		}
		while (robot_head_theta > 7) {
			turn(motorController, vis, robot_head_theta);
//			doMotion(vis, motorController);
			robot_head_theta = motorController.getHeadAngle();
		}
		theta = vis.getRobotTheta();
		std::cout << "My new theta is: " << theta << std::endl;
	}

	// TODO Add walking here

	std::cout << "Got behind ball!" << std::endl;
}
/*
 * This function returns true if it is ready to kick or false if something else happens
 */
/*bool*/void alignToKick(Vision &vis, MotorController &motorController) {
	vis.nextFrame();
	std::string motion = vis.getMotionRequest();
	while (motion != "KickL" && motion != "KickR") {
		motorController.setMotion(motion);
		doMotion(vis, motorController);
		motion = vis.getMotionRequest();
	}
	motorController.moveHead(MUL8_HEAD_UP, 500);
	motorController.setMotion(motion);
	doMotion(vis, motorController);
}

int main(int argc, char** argv) {
	MotorController motorController;
	motorController.init();
	motorController.setMotion("Wi");
	motorController.step(false);
	std::cout << "Made it here" << std::endl;
	Vision vis;
	vis.init(motorController);
	std::cout << "Created Vision Instance" << std::endl;
	std::string input;
	std::cout << "Robot theta = " << vis.getRobotTheta() << std::endl;
	std::cout << "What theta are we at? ";
	std::cin >> input;
	int initialTheta;
	std::stringstream(input)>>initialTheta;
	vis.updateRobotTheta(initialTheta);

	std::cout << "New Robot Theta: " << vis.getRobotTheta() << std::endl;
	search(vis);

	vis.setAction(SEARCH_FOR_BALL);

	walkTowardsBall(vis, motorController, 80);

	checkLocation(vis, motorController);

	std::cout << "Updated Robot Theta = " << vis.getRobotTheta() << std::endl;

	getBehindBall(vis, motorController);

	walkTowardsBall(vis, motorController, 70);

	alignToKick(vis, motorController);

//	motorController.setMotion("KickR");
//
//	doMotion(vis, motorController);

	std::cout << "Done!" << std::endl;

	motorController.stopHead();
	return 0;
}
